#include "robot_total_mode_task.h"
#include "OLED.h"
#include "bsp_buzzer.h"
#include "cmsis_os.h"
#include "detect_task.h"
#include "remote_control.h"
#include "robot_config.h"
#include "referee.h"
#include "CAN_receive.h"
#include "graphic_task.h"

//***************任务相关宏定义***************************//
#define ROBOT_MODE_TASK_TIME 10  // 10ms执行一次

//***************全局变量*******************************//

static int8_t robotAuxiliaryTarget = 0;             // 用于记录机器人的自瞄状态
static const RC_ctrl_t* rc_p;                       // 遥控器位置指针，需要初始化
static const remote_control_t* remoteControl_p; //图传链路控制
static uint8_t robotState = ROBOT_STATE_POWERLESS;  // 机器人初始状态
static uint8_t keyBoardAndMouseHasChanged = 0;      // 记录是否有键盘鼠标操作，避免遥控器的指令把键盘鼠标的指令覆盖了
#ifdef SENTRY
static betweenTwoSTM_t* betweenTwoSTMMessage_p;
#endif


void robot_total_mode_task(void const* pvParameters) {
    rc_p = get_remote_control_point();
    remoteControl_p=get_usart_remote_control_p();
    robotAuxiliaryTarget = 0;
    static int8_t lastRcMode;  // 记录遥控器值
    lastRcMode = rc_p->rc.ch[RC_TOTAL_MODE];
    #ifdef SENTRY
    betweenTwoSTMMessage_p = get_between_two_STM_message_point();
    #endif
    while (1) {
        #ifndef SENTRY_CHASSIS
        if (lastRcMode != rc_p->rc.ch[RC_TOTAL_MODE]) {
            keyBoardAndMouseHasChanged = 0;  // 再次使用遥控器
        }
        lastRcMode = rc_p->rc.ch[RC_TOTAL_MODE];
        if (!keyBoardAndMouseHasChanged)  // 键鼠状态清除
        {
            if (switch_is_up(rc_p->rc.ch[RC_TOTAL_MODE])) {
                robotState = ROBOT_STATE_SPINNER;
            } else if (switch_is_mid(rc_p->rc.ch[RC_TOTAL_MODE])) {
                robotState = ROBOT_STATE_COMMON;
            } else if (switch_is_down(rc_p->rc.ch[RC_TOTAL_MODE])) {
                robotState = ROBOT_STATE_AUTO;
            }
        }
        // 探测键盘鼠标状态
        if (remoteControl_p->right_button_down) {
            robotAuxiliaryTarget = 1;
            keyBoardAndMouseHasChanged = 1;
        }else{
            robotAuxiliaryTarget = 0;
        }
        if (remoteControl_p->keyboard_value  & ROBOT_STATE_TO_COMMON) {
            robotState = ROBOT_STATE_COMMON;
            keyBoardAndMouseHasChanged = 1;
        }
        if (remoteControl_p->keyboard_value  & ROBOT_STATE_TO_SPINNER) {
            robotState = ROBOT_STATE_SPINNER;
            keyBoardAndMouseHasChanged = 1;
        }
        // 掉线检测
        if (toe_is_error(MINI_PC_TOE)) {
            robotAuxiliaryTarget = 0;
        }
        if (toe_is_error(DBUS_TOE)) {
            robotState = ROBOT_STATE_POWERLESS;
        }
        if (toe_is_error(REFEREE_CONTROL_TOE))
        {
            keyBoardAndMouseHasChanged = 0;
            if(!game_not_started()){
                robotState = ROBOT_STATE_AUTO;
            }
        }
        
        // 强制无力
        if (switch_is_down(rc_p->rc.ch[RC_POWERLESS])) {
            robotState = ROBOT_STATE_POWERLESS;
        }
        if(rc_is_powerless()){
            robotState = ROBOT_STATE_AUTO;
        }
        #ifdef SENTRY
        betweenTwoSTMMessage_p->nextmode=robotState;
        #endif
        #else
        robotState=betweenTwoSTMMessage_p->nextmode;
        #endif
        osDelay(ROBOT_MODE_TASK_TIME);
    }
}

int8_t robot_is_auxiliaryTarget(void)
{
    return robotAuxiliaryTarget;
}

uint8_t * get_robot_present_mode(void)
{
    return &robotState;
}

uint8_t key_has_changed (void)
{
    return keyBoardAndMouseHasChanged;
}
